
#include "RobotAnimation.hpp"


NAMESPACE_BEGIN(NSRobotAnimation)

RobotAnimation* RobotAnimation::ins = nullptr;

void RobotAnimation::Initialize() {
	ani_timer = new QTimer(this);
	m_frame = 30;
	ani_timer->setInterval(1000 / m_frame);
	connect(ani_timer, &QTimer::timeout, this, &RobotAnimation::animationCallback);


}


void RobotAnimation::Finalize() {


}


void RobotAnimation::setFrame(int frame) {
	m_frame = frame;
	ani_timer->setInterval(1000 / m_frame);

}


void RobotAnimation::play() {
	ani_timer->start();

}



void RobotAnimation::setPlay(bool play) {
	if (play) {
		this->play();
		return;
	}
	this->stop();
}



void RobotAnimation::stop() {
	ani_timer->stop();

}



void RobotAnimation::rotateJoint(int robot_id, int joint_id, int deg, int frame) {
	NSRobotBase::RobotBase* robot = INSTANCE(RobotExchange)->getRobot(robot_id);
	if (!robot) return;
	int rot = 0;
	int step = 1;
	QTimer* timer = new QTimer(this);
	timer->setInterval(int(1000 / frame));
	 connect(timer, &QTimer::timeout, this, [robot, joint_id, deg, timer, rot, step, this]() mutable {
		const double* zdir = robot->getTopoDatas(joint_id).zdir;
		SP(vtkAssembly, joint) = robot->getJoint(joint_id);
		joint->RotateX(step* (deg /abs(deg)) * zdir[0]);
		joint->RotateY(step* (deg /abs(deg)) * zdir[1]);
		joint->RotateZ(step* (deg /abs(deg)) * zdir[2]);
		emit requireRenderSignal();
		if (rot >= abs(deg)-1) {
			timer->stop();
			timer->deleteLater();
		}
		rot += step;
		printf("rot %d\n", rot);
		});
	timer->start();

}



void RobotAnimation::animationCallback() {
	static int rot = 0;
	NSRobotBase::RobotBase* robot = INSTANCE(RobotExchange)->getRobot(0);
	if (robot) {
		for (int i = 1; i < 7; i++) {
			double value = -rot * i / 10.0;
			const double* zdir = robot->getTopoDatas(i).zdir;
			SP(vtkAssembly, joint) = robot->getJoint(i);
			joint->SetOrientation(value * zdir[0], value * zdir[1], value * zdir[2]);

		}

		rot++;
		emit requireRenderSignal();
	}

}

NAMESPACE_END

